#include "stdafx.h"
#include "PhysEngine.h"
#include <algorithm>

PhysEngine::PhysEngine()
{
	collisionConfiguration.reset(new btDefaultCollisionConfiguration());
	dispatcher.reset(new btCollisionDispatcher(collisionConfiguration.get()));
	broadphase.reset(new btDbvtBroadphase());
	solver.reset(new btSequentialImpulseConstraintSolver());
	world.reset(new btDiscreteDynamicsWorld(dispatcher.get(), broadphase.get(), solver.get(), collisionConfiguration.get()));
	world->setGravity(btVector3(0.f, -100.f, 0.f));
}

btDiscreteDynamicsWorld* PhysEngine::getWorld()
{
	return world.get();
}

void PhysEngine::step(float dt)
{
	world->stepSimulation(dt, 10);
}

btRigidBody* PhysEngine::createBody(btCollisionShape* shape, const btTransform& transform, float mass)
{
	if(std::find(collisionShapes.begin(), collisionShapes.end(), shape) == collisionShapes.end())
		collisionShapes.push_back(shape);
	bool isDynamic = (mass != 0.f);
	btVector3 localInertia(0,0,0);
	if(isDynamic) shape->calculateLocalInertia(mass, localInertia);
	btMotionState* motionState = new btDefaultMotionState(transform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, motionState, shape, localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	body->setFriction(3.5f);
	world->addRigidBody(body);
	return body;
}
void PhysEngine::addJoint(btTypedConstraint* joint, bool disableCollision)
{
	world->addConstraint(joint, disableCollision);
}

void PhysEngine::reset()
{

	int numConstraints = world->getNumConstraints();
	for (int i = 0; i < numConstraints; i++)
	{
		world->getConstraint(i)->setEnabled(true);
	}

	int numObjects = world->getNumCollisionObjects();

	btCollisionObjectArray copyArray = world->getCollisionObjectArray();

	for (int i = 0; i < numObjects; i++)
	{
		btCollisionObject* colObj = copyArray[i];
		btRigidBody* body = btRigidBody::upcast(colObj);
		if(body)
		{
			if (body->getMotionState())
			{
				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
				myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
				body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans );
				colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
				colObj->forceActivationState(ACTIVE_TAG);
				colObj->activate();
				colObj->setDeactivationTime(0);
			}
			if (world->getBroadphase()->getOverlappingPairCache())
				world->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(), world->getDispatcher());

			btRigidBody* body = btRigidBody::upcast(colObj);
			if (body && !body->isStaticObject())
			{
				btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0));
				btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0));
			}
		}

	}

	world->getBroadphase()->resetPool(world->getDispatcher());
	world->getConstraintSolver()->reset();
}

void PhysEngine::removeBody(btRigidBody* body)
{
	world->removeRigidBody(body);
}

PhysEngine::~PhysEngine()
{
	for (int i = 0; i < world->getNumCollisionObjects(); i++)
	{
		btCollisionObject* obj = world->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		world->removeCollisionObject(obj);
		delete obj;
	}

	for (size_t i = 0; i < collisionShapes.size(); i++)
	{
		btCollisionShape* shape = collisionShapes[i];
		delete shape;
	}
}